import java.io.IOException;
import java.util.Vector;

import javax.xml.stream.XMLStreamException;

import lejos.nxt.Sound;

/**
 * Contains the main class for the robot.
 * 
 * @author bjorn, kristian
 * 
 */
public class Robot {

	private static BTConnection btc = new BTConnection();
	private static Boolean connected = false;
	
	/*
	private static void revertState() throws XMLStreamException, IOException {
			XMLHandler newXML = new XMLHandler();
				if (returnXML.isMove(0)) {
					newXML.addCommand(new ServerCommand("move", new CommandParameter("req", returnXML.getValue(0, "req") * -1)));
				} else if (returnXML.isRotate(0)) {
					newXML.addCommand(new ServerCommand("rotate", new CommandParameter("req", returnXML.getValue(0, "req") * -1)));
				}
				readAndAct(newXML.toString("s2r"));
			}
			*/	
	/**
	 * Acts on the commands in the buffer.
	 * 
	 * @param buffer
	 * @throws XMLStreamException
	 * @throws IOException
	 */
	private static void readAndAct(String buffer) throws XMLStreamException,
			IOException {

		XMLHandler inputXML = new XMLHandler();
		Vector<CommandParameter> tempParam = new Vector<CommandParameter>();
		Vector<Integer> scanResult = new Vector<Integer>();
		XMLHandler returnXML = new XMLHandler();

		// First check that we actually have new data.
		if (!buffer.isEmpty()) {

			// Play a feedback sound.
			Sound.beep();

			// Read the data into the XML handler.
			inputXML.fromXML(buffer);

			// Loop through all commands in the buffer.
			for (int i = 0; i < inputXML.getCommand().size(); i++) {

				// tempParam is a vector of parameters to send with the current
				// command. It should be cleared before starting with the next
				// command.
				tempParam.clear();

				// This is for feedback, print the command and parameter on the
				// LCD.
				System.out.println(inputXML.getName(i) + ": "
						+ inputXML.getValue(i, "req"));

				// Start acting on the different commands:

				if (inputXML.isMove(i)) {
					// The current command is move Move.

					// Do the movement
					int cms = Movement.move(inputXML.getValue(i, "req"));

					// Add the req and real parameter to the command.
					tempParam.addElement(new CommandParameter("req", inputXML
							.getValue(i, "req")));
					tempParam.addElement(new CommandParameter("real", cms));

					// Add the ACK command to the list of commands to be sent
					// back.
					returnXML.addCommand(new ServerCommand("move", tempParam));

				} else if (inputXML.isRotate(i)) {
					// The current command is rotate

					// Do the rotation.
					Movement.rotate(inputXML.getValue(i, "req"));

					// Add the req and real parameter to the command.
					tempParam.addElement(new CommandParameter("req", inputXML
							.getValue(i, "req")));
					tempParam.addElement(new CommandParameter("real", inputXML
							.getValue(i, "req")));

					// Add the ACK command to the list of commands to be sent
					// back.
					returnXML
							.addCommand(new ServerCommand("rotate", tempParam));

				} else if (inputXML.isScan(i)) {
					// The current command is scan.

					// Do the scan
					scanResult = USSensor.scan(inputXML.getValue(i, "req"));

					// Add the parameters for the ACK command.
					tempParam.addElement(new CommandParameter("n", inputXML
							.getValue(i, "req")));
					for (Integer scanI = 0; scanI < scanResult.size(); scanI++) {
						tempParam.addElement(new CommandParameter("n"
								+ ((Integer) (scanI + 1)).toString(),
								scanResult.elementAt(scanI)));
					}

					// Add the ACK command to the list of commands to send.
					returnXML.addCommand(new ServerCommand("scan", tempParam));

					// Clear the scan results, in case we get another scan
					// command.
					scanResult.clear();
				} else if (inputXML.isReboot(i)) {
					// The current command is reboot.

					// Send ack.
					returnXML.addCommand(new ServerCommand("reboot",
							new CommandParameter("req", 0)));

					// TODO: Implement reboot.
					LCDDisplay.print("IMPLEMENT REBOOT.");
				} else if (inputXML.isClose(i)) {
					// The current command is G2close.

					// Send ack.
					returnXML.addCommand(new ServerCommand("G2close",
							new CommandParameter("req", 0)));

					// Close connection.
					LCDDisplay.print("Close connection");
					btc.close();
					connected = false;
				} /*
				 * else if (inputXML.isAuto(i)) { while(true) { scanResult =
				 * USSensor.scan(16); int max = 0; for(int it = 0; it <
				 * scanResult.size(); it++) { if(scanResult.elementAt(it) > max)
				 * max = scanResult.elementAt(it); } // tempParam.addElement(new
				 * CommandParameter("n", inputXML // .getValue(16, "req"))); //
				 * for (Integer scanI = 0; scanI < scanResult.size(); scanI++) {
				 * // tempParam // .addElement(new CommandParameter("n" // +
				 * scanI.toString(), scanResult // .elementAt(scanI))); // } //
				 * returnXML.addCommand(new ServerCommand("scan", tempParam));
				 * // scanResult.clear(); // if (connected) { //
				 * btc.send(returnXML.toString("r2s")); // }
				 * 
				 * // tempParam.clear(); int temp =
				 * (360/scanResult.size())*scanResult.indexOf(max);
				 * Movement.rotate(temp); scanResult.clear(); //
				 * tempParam.addElement(new CommandParameter("req", inputXML //
				 * .getValue(temp, "req"))); // tempParam.addElement(new
				 * CommandParameter("real", inputXML // .getValue(temp,
				 * "req"))); // returnXML.addCommand(new ServerCommand("rotate",
				 * tempParam)); // if (connected) { //
				 * btc.send(returnXML.toString("r2s")); // } // //
				 * tempParam.clear(); Movement.move(17);
				 * tempParam.addElement(new CommandParameter("req", inputXML
				 * .getValue(17, "req"))); tempParam.addElement(new
				 * CommandParameter("real", inputXML .getValue(17, "req")));
				 * returnXML.addCommand(new ServerCommand("move", tempParam));
				 * }}
				 */
			}
			if (connected) {
				btc.send(returnXML.toString("r2s"));
			}
		}
	}

	/**
	 * Main class that handles robot operations.
	 * 
	 * @param args
	 * @throws Exception
	 */
	public static void main(String[] args) {

		while (true) {
			try {
				btc.waitForConnection();
				connected = true;
				while (connected) {
					try {
						readAndAct(btc.recieveData());
					} catch (IOException e) {
						btc.close();
						connected = false;
						LCDDisplay.print("Connection lost.");
					} catch (XMLStreamException e) {
						btc.close();
						connected = false;
						LCDDisplay.print("Malformed XML.\nConnection lost.");
					}
				}

			} catch (Exception e) {
				try {
					btc.close();
				} catch (IOException exc) {
					// Do nothing.
				}
				connected = false;
				LCDDisplay.print("Unhandled error:\n" + e.getMessage());
			}

		}
	}
}